#include <iostream>
#include <fstream>
#include <limits>
#include <cmath>
#include <vector>
#include <algorithm>
#include "PriorityQueue.h"
using namespace std;
using std::pair;

template <typename Name >

class AlgStar {
private:
	double (*evristic)(typename BiGraph<Name>::Vertex*, typename BiGraph<Name>::Vertex*);
public:
	AlgStar(double (*evr)(typename BiGraph<Name>::Vertex*, typename BiGraph<Name>::Vertex*)) {
		evristic=evr;
	};
	~AlgStar() {};
	void relax(typename BiGraph<Name>::Vertex* v, typename BiGraph<Name>::Vertex* end, BiGraph<Name>& BGraph, PriorityQueue<double, typename BiGraph<Name>::Vertex*>& open, vector<typename BiGraph<Name>::Vertex*>& closed , map<Name, Name>& parent, map<typename BiGraph<Name>::Vertex*, double>& g, map<typename BiGraph<Name>::Vertex*, double>& h);	
	vector<Name> astar(BiGraph<Name>& BGraph, typename BiGraph<Name>::Vertex* start, typename BiGraph<Name>::Vertex* end);	
};


template <typename Name>
void AlgStar<Name>::relax(typename BiGraph<Name>::Vertex* v, typename BiGraph<Name>::Vertex* end, BiGraph<Name>& BGraph, PriorityQueue<double, typename BiGraph<Name>::Vertex*>& open, vector<typename BiGraph<Name>::Vertex*>& closed , map<Name, Name>& parent, map<typename BiGraph<Name>::Vertex*, double>& g, map<typename BiGraph<Name>::Vertex*, double>& h)
		 {
			 pair<BiGraph<Name>::out_edge_iterator, BiGraph<Name>::out_edge_iterator> edges=v->out_edge();
			 for (BiGraph<Name>::out_edge_iterator itr_e=edges.first; itr_e!=edges.second; itr_e++) {
				 if (itr_e->get_weight() < numeric_limits<double>::max()) {
					 BiGraph<Name>::Vertex* u=BGraph.find_or_create_vertex((itr_e->get_name()).second);
					 if (find(closed.begin(), closed.end(), u)==closed.end()) {
						 if (open.containValue(u)) {
							 
							 if (g[u]>g[v]+(itr_e->get_weight())) {
								 g[u]=g[v]+(itr_e->get_weight());
								 open.changeKey(g[u]+h[u], u);
								 parent[u->get_name()]=v->get_name();
							 }
						 } else {
							 g[u]=g[v]+(itr_e->get_weight());
							 h[u]=evristic(u, end);
							 open.push(g[u]+h[u], u);
							 parent[u->get_name()]=v->get_name();
						 }
					 }
				 }
			 }
		 }

template <typename Name>
vector<Name> AlgStar<Name>::astar(BiGraph<Name>& BGraph, typename BiGraph<Name>::Vertex* start, typename BiGraph<Name>::Vertex* end)
		{
			BiGraph<Name>::vertices_range vertices=BGraph.vertices();
			PriorityQueue<double, BiGraph<Name>::Vertex*> open;
			map<BiGraph<Name>::Vertex*, double> g;
			map<Name, Name> parent;
			vector<BiGraph<Name>::Vertex*> closed;
			map<BiGraph<Name>::Vertex*, double> h;
			Name endName=end->get_name();
			Name startName=start->get_name();
			open.push(0, start);
			g[start]=0;
			parent[startName]=startName;
			h[start]=evristic(start, end);
			Name name=startName;
			while (!open.empty())
			{
				BiGraph<Name>::Vertex* v=open.pop();
				if (v->get_name()==endName) {
					break;
				}
				relax(v, end, BGraph, open, closed, parent, g, h);
				
				open.deleteValue(v);
				
				closed.push_back(v);
			}
			vector<Name> path;
			if (parent.find(endName)!=parent.end()) {
				while (endName!=startName) {
					path.push_back(endName);
					endName=parent[endName];
				}
				path.push_back(startName);
				reverse(path.begin(), path.end());
			}
			return path;
		}